#include "queue.h"

#include "queue.h"
#include <string.h>

bool queue_init(Queue_HandleTypeDef *q, uint8_t *buffer, uint16_t capacity, uint16_t item_size)
{
    if (!q || !buffer || capacity == 0 || item_size == 0)
        return false;

    q->buffer = buffer;
    q->capacity = capacity;
    q->item_size = item_size;
    q->head = 0;
    q->tail = 0;
    q->count = 0;

    return true;
}

bool queue_is_empty(Queue_HandleTypeDef *q)
{
    return q->count == 0;
}

bool queue_is_full(Queue_HandleTypeDef *q)
{
    return q->count == q->capacity;
}

uint16_t queue_count(Queue_HandleTypeDef *q)
{
    return q->count;
}

bool queue_push(Queue_HandleTypeDef *q, const void *item)
{
    if (queue_is_full(q) || item == NULL)
        return false;

    uint8_t *dst = q->buffer + (q->tail * q->item_size);
    memcpy(dst, item, q->item_size);

    q->tail = (q->tail + 1) % q->capacity;
    q->count++;

    return true;
}

bool queue_pop(Queue_HandleTypeDef *q, void *item)
{
    if (queue_is_empty(q) || item == NULL)
        return false;

    uint8_t *src = q->buffer + (q->head * q->item_size);
    memcpy(item, src, q->item_size);

    q->head = (q->head + 1) % q->capacity;
    q->count--;

    return true;
}




